copp\copp\copp2/
formulation.rs

1//! Problem data models and builders for second-order path parameterization.
2//!
3//! # Method identity
4//! This module defines validated formulation objects for:
5//! - **Time-Optimal Path Parameterization (TOPP2)**,
6//! - **Convex-Objective Path Parameterization (COPP2)**.
7//!
8//! # Discrete variables (shared notation)
9//! On a station grid with closed index interval `[idx_s_start, idx_s_final]`:
10//! - state profile is `a(s)=\dot{s}^2`;
11//! - boundary tuple is `a_boundary = (a_start, a_final)`;
12//! - station count is `s_len = idx_s_final - idx_s_start + 1`.
13//!
14//! # High-level pipeline
15//! 1. Construct [`Topp2ProblemBuilder`](crate::solver::topp2_ra::Topp2ProblemBuilder) or [`Copp2ProblemBuilder`](crate::solver::copp2_socp::Copp2ProblemBuilder) from caller data.
16//! 2. Run builder validation (index interval, bounds, objective compatibility).
17//! 3. Build immutable problem objects used by DP/optimization backends.
18
19use crate::copp::constraints::Constraints;
20use crate::copp::{CoppObjective, validate_copp2_objectives};
21use crate::diag::{CoppError, check_non_negative, check_s_interval_valid};
22use crate::robot::robot_core::{Robot, RobotBasic, RobotTorque};
23
24/// Formulated TOPP2 problem data.
25///
26/// # Fields
27/// - `constraints`: path-dependent kinematic/dynamic bounds on the selected interval;
28/// - `idx_s_interval`: closed station-index interval `[idx_s_start, idx_s_final]`;
29/// - `a_boundary`: endpoint state tuple `(a_start, a_final)`.
30pub struct Topp2Problem<'a> {
31    pub(crate) constraints: &'a Constraints,
32    pub(crate) idx_s_interval: (usize, usize),
33    pub(crate) a_boundary: (f64, f64),
34}
35
36impl<'a> Topp2Problem<'a> {
37    /// Return station count on the closed interval.
38    ///
39    /// For `[idx_s_start, idx_s_final]`, this returns
40    /// `idx_s_final - idx_s_start + 1`.
41    #[inline]
42    pub fn s_len(&self) -> usize {
43        self.idx_s_interval.1 - self.idx_s_interval.0 + 1
44    }
45}
46
47/// Builder for [`Topp2Problem`](crate::solver::topp2_ra::Topp2Problem).
48///
49/// # Example
50/// The example below builds the smallest TOPP2 problem from a robot's station grid.
51///
52/// ```rust
53/// # fn main() -> Result<(), copp::diag::CoppError> {
54/// use copp::robot::Robot;
55/// use copp::solver::topp2_ra::Topp2ProblemBuilder;
56///
57/// let mut robot = Robot::with_capacity(2usize, 3);
58/// let s = [0.0, 0.5, 1.0];
59/// robot.with_s(s.as_slice())?;
60///
61/// let problem = Topp2ProblemBuilder::new(&robot, (0, 2), (0.0, 0.0)).build()?;
62/// assert_eq!(problem.s_len(), 3);
63/// # Ok(())
64/// # }
65/// ```
66pub struct Topp2ProblemBuilder<'a> {
67    /// Reference to path constraints.
68    pub constraints: &'a Constraints,
69    /// Closed station-index interval `(idx_s_start, idx_s_final)`.
70    pub idx_s_interval: (usize, usize),
71    /// Endpoint state tuple `(a_start, a_final)`.
72    pub a_boundary: (f64, f64),
73}
74
75impl<'a> Topp2ProblemBuilder<'a> {
76    /// Create a TOPP2 builder from a [`Robot`](crate::robot::Robot) reference.
77    ///
78    /// # Parameters
79    /// - `robot`: robot wrapper with the trait [`RobotBasic`](crate::robot::RobotBasic) whose constraint buffer defines the problem domain.
80    /// - `idx_s_interval`: closed station-index interval `(idx_s_start, idx_s_final)`.
81    /// - `a_boundary`: endpoint state tuple `(a_start, a_final)`.
82    #[inline]
83    pub fn new<M: RobotBasic>(
84        robot: &'a Robot<M>,
85        idx_s_interval: (usize, usize),
86        a_boundary: (f64, f64),
87    ) -> Self {
88        Self {
89            constraints: &robot.constraints,
90            idx_s_interval,
91            a_boundary,
92        }
93    }
94
95    /// Create a TOPP2 builder with all required fields.
96    ///
97    /// # Parameters
98    /// - `constraints`: reference to path constraints defining the problem domain.
99    /// - `idx_s_interval`: closed station-index interval `(idx_s_start, idx_s_final)`.
100    /// - `a_boundary`: endpoint state tuple `(a_start, a_final)`.
101    #[inline]
102    pub fn with_constraint(
103        constraints: &'a Constraints,
104        idx_s_interval: (usize, usize),
105        a_boundary: (f64, f64),
106    ) -> Self {
107        Self {
108            constraints,
109            idx_s_interval,
110            a_boundary,
111        }
112    }
113
114    /// Build a validated [`Topp2Problem`](crate::solver::topp2_ra::Topp2Problem).
115    #[inline]
116    pub fn build(&self) -> Result<Topp2Problem<'a>, CoppError> {
117        self.validate()?;
118        Ok(Topp2Problem {
119            constraints: self.constraints,
120            idx_s_interval: self.idx_s_interval,
121            a_boundary: self.a_boundary,
122        })
123    }
124
125    /// Validate builder fields and consistency.
126    #[inline]
127    pub fn validate(&self) -> Result<(), CoppError> {
128        check_s_interval_valid(
129            "Topp2ProblemBuilder",
130            self.idx_s_interval.0,
131            self.idx_s_interval.1,
132        )?;
133        self.constraints.check_s_in_bounds(
134            self.idx_s_interval.0,
135            self.idx_s_interval.1 - self.idx_s_interval.0 + 1,
136        )?;
137        check_non_negative(
138            "Topp2ProblemBuilder",
139            "a_start (a_boundary.0)",
140            self.a_boundary.0,
141        )?;
142        check_non_negative(
143            "Topp2ProblemBuilder",
144            "a_final (a_boundary.1)",
145            self.a_boundary.1,
146        )?;
147        Ok(())
148    }
149}
150
151/// Formulated COPP2 problem data.
152///
153/// # Fields
154/// - `robot`: robot model supplying constraints and torque-related terms;
155/// - `objectives`: objective list for COPP2 optimization;
156/// - `idx_s_interval`: closed station-index interval `[idx_s_start, idx_s_final]`;
157/// - `a_boundary`: endpoint state tuple `(a_start, a_final)`.
158pub struct Copp2Problem<'a, M: RobotTorque> {
159    pub(crate) robot: &'a Robot<M>,
160    pub(crate) objectives: &'a [CoppObjective<'a>],
161    pub(crate) idx_s_interval: (usize, usize),
162    pub(crate) a_boundary: (f64, f64),
163}
164
165impl<'a, M: RobotTorque> Copp2Problem<'a, M> {
166    /// Return station count on the closed interval.
167    ///
168    /// For `[idx_s_start, idx_s_final]`, this returns
169    /// `idx_s_final - idx_s_start + 1`.
170    #[inline]
171    pub fn s_len(&self) -> usize {
172        self.idx_s_interval.1 - self.idx_s_interval.0 + 1
173    }
174}
175
176/// Builder for [`Copp2Problem`](crate::solver::copp2_socp::Copp2Problem).
177///
178/// # Example
179/// The example below builds a COPP2 problem with time and thermal objectives.
180///
181/// ```rust
182/// # fn main() -> Result<(), copp::diag::CoppError> {
183/// use copp::robot::Robot;
184/// use copp::solver::copp2_socp::{Copp2ProblemBuilder, CoppObjective};
185///
186/// let mut robot = Robot::with_capacity(2, 3);
187/// let s = [0.0, 0.5, 1.0];
188/// robot.with_s(s.as_slice())?;
189///
190/// let normalize = [1.0, 1.0];
191/// let objectives = [
192///     CoppObjective::Time(1.0),
193///     CoppObjective::ThermalEnergy(0.1, &normalize),
194/// ];
195///
196/// let problem =
197///     Copp2ProblemBuilder::new(&robot, (0, 2), (0.0, 0.0), &objectives).build()?;
198/// assert_eq!(problem.s_len(), 3);
199/// # Ok(())
200/// # }
201/// ```
202pub struct Copp2ProblemBuilder<'a, M: RobotTorque> {
203    /// Reference to robot model defining constraints and dynamics.
204    pub robot: &'a Robot<M>,
205    /// Closed station-index interval `(idx_s_start, idx_s_final)`.
206    pub idx_s_interval: (usize, usize),
207    /// Endpoint state tuple `(a_start, a_final)`.
208    pub a_boundary: (f64, f64),
209    /// Objectives for COPP2 optimization.
210    pub objectives: &'a [CoppObjective<'a>],
211}
212
213impl<'a, M: RobotTorque> Copp2ProblemBuilder<'a, M> {
214    /// Create a COPP2 builder with all required fields.
215    ///
216    /// # Parameters
217    /// - `robot`: robot with the trait [`RobotTorque`](crate::robot::RobotTorque) defining the problem domain.
218    /// - `idx_s_interval`: closed station-index interval `(idx_s_start, idx_s_final)`.
219    /// - `a_boundary`: endpoint state tuple `(a_start, a_final)`.
220    #[inline]
221    pub fn new(
222        robot: &'a Robot<M>,
223        idx_s_interval: (usize, usize),
224        a_boundary: (f64, f64),
225        objectives: &'a [CoppObjective<'a>],
226    ) -> Self {
227        Self {
228            robot,
229            idx_s_interval,
230            a_boundary,
231            objectives,
232        }
233    }
234
235    /// Build a validated [`Copp2Problem`](crate::solver::copp2_socp::Copp2Problem).
236    #[inline]
237    pub fn build(&self) -> Result<Copp2Problem<'a, M>, CoppError> {
238        self.validate()?;
239        Ok(Copp2Problem {
240            robot: self.robot,
241            idx_s_interval: self.idx_s_interval,
242            a_boundary: self.a_boundary,
243            objectives: self.objectives,
244        })
245    }
246
247    /// Validate builder fields and objective compatibility.
248    #[inline]
249    pub fn validate(&self) -> Result<(), CoppError> {
250        check_s_interval_valid(
251            "Copp2ProblemBuilder",
252            self.idx_s_interval.0,
253            self.idx_s_interval.1,
254        )?;
255        self.robot.constraints.check_s_in_bounds(
256            self.idx_s_interval.0,
257            self.idx_s_interval.1 - self.idx_s_interval.0 + 1,
258        )?;
259        check_non_negative(
260            "Copp2ProblemBuilder",
261            "a_start (a_boundary.0)",
262            self.a_boundary.0,
263        )?;
264        check_non_negative(
265            "Copp2ProblemBuilder",
266            "a_final (a_boundary.1)",
267            self.a_boundary.1,
268        )?;
269
270        let s_len = self.idx_s_interval.1 - self.idx_s_interval.0 + 1;
271        validate_copp2_objectives(
272            "Copp2ProblemBuilder",
273            self.objectives,
274            self.robot.dim(),
275            s_len,
276        )?;
277        Ok(())
278    }
279}
280
281impl<'a, M: RobotTorque> Copp2Problem<'a, M> {
282    /// Convert to the TOPP2 view that shares interval and boundary fields.
283    ///
284    /// This is used by internal stages that only need standard TOPP2 constraints.
285    pub(crate) fn as_topp2_problem(&self) -> Topp2Problem<'a> {
286        Topp2Problem {
287            constraints: &self.robot.constraints,
288            idx_s_interval: self.idx_s_interval,
289            a_boundary: self.a_boundary,
290        }
291    }
292}
copp\copp\copp2/formulation.rs

copp\copp\copp2/
formulation.rs
